typedef struct quaternion {double w; vector v;} quaternion;

#define unit_quaternion(angle, vec, q)\
{\
  double length = vlength(vec);\
  (q).w = cos((angle) / 2.0);\
  if (length != 0.0) {\
    vscale1(sin((angle) / 2.0) / length, (vec), (q).v);\
  }\
  else {\
    vset((vec), (q).v);\
  }\
}


#define qscale(c , q1, q2)\
{\
  double cc = (c);\
  (q2).w = (q1).w * cc;\
  (q2).v.x = (q1).v.x * cc;\
  (q2).v.y = (q1).v.y * cc;\
  (q2).v.z = (q1).v.z * cc;\
}

#define qinc(q1, q2)\
  (q2).w = (q2).w + (q1).w;\
  (q2).v.x = (q2).v.x + (q1).v.x;\
  (q2).v.y = (q2).v.y + (q1).v.y;\
  (q2).v.z = (q2).v.z + (q1).v.z;

#define qdec(q1, q2)\
  (q2).w = (q2).w - (q1).w;\
  (q2).v.x = (q2).v.x - (q1).v.x;\
  (q2).v.y = (q2).v.y - (q1).v.y;\
  (q2).v.z = (q2).v.z - (q1).v.z;

#define qzeroize(q)\
(q).w = (double) 0.0;\
vzeroize((q).v);\

#define qset(q1, q2)\
  (q2).w = (q1).w;\
  (q2).v.x = (q1).v.x;\
  (q2).v.y = (q1).v.y;\
  (q2).v.z = (q1).v.z

#define qnormalize(q)\
{\
  double s = 1.0 / sqrt((q).w * (q).w + vdot((q).v, (q).v));\
  (q).w = s * (q).w;\
  vscale(s, (q).v, (q).v);\
}

#define qmult(q1, q2, q)\
{\
  vector temp1, temp2;\
  vcross((q1).v, (q2).v, temp2);\
  vscale((q1).w, (q2).v, temp1);\
  vinc(temp1, temp2);\
  vscale((q2).w, (q1).v, temp1);\
  vplus(temp1, temp2, (q).v);\
  (q).w = (q1).w * (q2).w - vdot((q1).v, (q2).v);\
}
  
#define unit_quaternion_to_rotation_matrix(q, m)\
{\
  double x2 = (q).v.x+(q).v.x, y2 = (q).v.y+(q).v.y, z2 = (q).v.z+(q).v.z;\
  double wx2 = x2*(q).w, xx2 = x2*(q).v.x, xy2 = x2*(q).v.y, xz2 = x2*(q).v.z;\
  double wy2 = y2*(q).w, yy2 = y2*(q).v.y, yz2 = y2*(q).v.z;\
  double wz2 = z2*(q).w, zz2 = z2*(q).v.z;\
  (m)[0][0] = 1.0 - yy2 - zz2; (m)[0][1] = xy2 - wz2; (m)[0][2] = xz2 + wy2;\
  (m)[1][0] = xy2 + wz2; (m)[1][1] = 1.0 - xx2 - zz2; (m)[1][2] = yz2 - wx2;\
  (m)[2][0] = xz2 - wy2; (m)[2][1] = yz2 + wx2; (m)[2][2] = 1.0 - xx2 - yy2;\
}
  
